package kit.edu.lego.gruppe3.levels;

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.subsumption.Behavior;

public class HitWall implements Behavior{

	public HitWall(SensorPort port ){
        sonar = new UltrasonicSensor(port );
    }
	
    private boolean suppressed = false;
    private TouchSensor touch;
    private UltrasonicSensor sonar;

    @Override
	public boolean takeControl() {
		return touch.isPressed() || sonar.getDistance() < 25;
	}
	
    @Override
	public void suppress() {
		suppressed = true;
	}
	
	@Override
	public void action() {
		
		  // Back up  and turn
		
		suppressed = false;
		
		Motor.A.rotate(-180,true);
		Motor.C.rotate(-360,true);
		
		while( Motor.C.isMoving() & !suppressed )
			Thread.yield();
		
		Motor.A.stop();
		Motor.C.stop();
	
	}  
}
